#coding:utf-8
import os
import ConfigParser
import numpy as np
from optparse import OptionParser
import cv2
import glob

def parse_args():
    parser = OptionParser()
    parser.add_option('-c', '--conf', dest="iniFile", default=None, help="ini file")
    parser.add_option('-i', '--image', dest='imgName', default=None, help="image name")
    parser.add_option('-p', '--path', dest='path', default = None, help="image path")
    parser.add_option('-s', '--spath', dest='savepath', default=None, help="save path")
    parser.add_option('-d', '--dsample', dest='dsample',default=1, help="down sample ratio")
    options, args = parser.parse_args()
    print 'config file', options.iniFile
    return options

def load_ini(iniFile):
    cf = ConfigParser.ConfigParser()
    cf.read(iniFile)
    confs = {}
    confs['fx'] = cf.getfloat('cameraCamConf', 'fx')
    confs['fy'] = cf.getfloat('cameraCamConf', 'fy')
    confs['u0'] = cf.getfloat('cameraCamConf', 'u0')
    confs['v0'] = cf.getfloat('cameraCamConf', 'v0')
    confs['k1'] = cf.getfloat('cameraCamConf', 'k1')
    confs['k2'] = cf.getfloat('cameraCamConf', 'k2')
    confs['k3'] = cf.getfloat('cameraCamConf', 'k3')
    confs['k4'] = cf.getfloat('cameraCamConf', 'k4')
    confs['imageWidth'] = cf.getint('cameraCamConf', 'imageWidth')
    confs['imageHeight'] = cf.getint('cameraCamConf', 'imageHeight')
    confs['scale'] = cf.getfloat('processConf', 'scale')
    return confs

if __name__ == '__main__':
    opts = parse_args()
    confs = load_ini(opts.iniFile)
    K = np.array([[confs['fx'], 0.0, confs['u0']],
                  [0.0, confs['fy'], confs['v0']],
                  [0.0, 0.0, 1.0]], dtype=np.float32)
    D = np.array([confs['k1'], confs['k2'],
                  confs['k3'], confs['k4']],dtype=np.float32)
    KNew = np.array([[K[0,0]*confs['scale'], 0.0, confs['imageWidth']*confs['scale']/2],
                     [0.0, K[0,0]*confs['scale'], confs['imageHeight']*confs['scale']/2],
                     [0.0, 0.0, 1.0]], dtype=np.float32)
    R = np.eye(3, dtype=np.float32)
    map1, map2 = cv2.fisheye.initUndistortRectifyMap(K, D, R, KNew,
                                                     (int(confs['imageWidth']*confs['scale']), int(confs['imageHeight']*confs['scale'])),
                                                     cv2.CV_16SC2)
    if opts.path is None:
        img = cv2.imread(opts.imgName)
        img_rec = cv2.remap(img, map1, map2, cv2.INTER_LINEAR, borderMode=cv2.BORDER_CONSTANT)
        cv2.imwrite('rectified2.jpg', img_rec)
    else:
        file_names = glob.glob(os.path.join(opts.path, '*.jpg'))
        file_names = sorted(file_names)
        for k in range(0,len(file_names),int(opts.dsample)):
            n = file_names[k]
            img = cv2.imread(n)
            pure_name = os.path.split(n)[1].split('.')[0]
            img_rec = cv2.remap(img, map1, map2, cv2.INTER_LINEAR, borderMode=cv2.BORDER_CONSTANT)
            cv2.imwrite(os.path.join(opts.savepath, pure_name+'.jpg'), img_rec)

